#include "pastycam.h"
#include <cmath>
#include "OpenGLHelpers.h"

PastyCam::PastyCam()
{
	posx=posy=posz=hang=vang = 0.0;
	lastDx=lastDy=lastDz = 0.0f;

	float identity[16] = {
		1,0,0,0,
		0,1,0,0,
		0,0,1,0,
		0,0,0,1,
	};
	for(int i=0; i<16; i++){
		modelview[i] = identity[i];
	}
	
}

void PastyCam::update(float vx, float vy, float vz, float deltaHang, float deltaVang)
{
	float eps = 1e-20f;
	if( fabs(vx)<eps && fabs(vy)<eps && fabs(vz)<eps && fabs(deltaHang)<eps && fabs(deltaVang)<eps ) return;
	lastDx = vx;
	lastDy = vy;
	lastDz = vz;

	hang += deltaHang;
	vang += deltaVang;

	float ch = cos(hang);
	float cv = cos(vang);
	float sh = sin(hang);
	float sv = sin(vang);

	float rotMat[16];
	rotMat[0] = ch;  rotMat[2] = cv*sh;		
	rotMat[4] = 0;	 rotMat[6] = sv;		  
	rotMat[8] =-sh;  rotMat[10] = cv*ch;	
	// ydir=zdir cross xdir:
	rotMat[1] = rotMat[ 6]*rotMat[ 8] - rotMat[10]*rotMat[ 4]; // c.x=a.y*b.z-a.z*b.y
	rotMat[5] = rotMat[10]*rotMat[ 0] - rotMat[ 2]*rotMat[ 8]; // c.y=a.z*b.x-a.x*b.z
	rotMat[9] = rotMat[ 2]*rotMat[ 4] - rotMat[ 6]*rotMat[ 0]; // c.z=a.x*b.y-a.y*b.x

	rotMat[3] = 0.0f; 
	rotMat[7] = 0.0f; 
	rotMat[11] = 0.0f;
	rotMat[12] = 0.0f; rotMat[13] = 0.0f; rotMat[14] = 0.0f; rotMat[15] = 1.0f;
	
	// Move cam in view direction
	float viewDx = rotMat[0]*vx + rotMat[1]*vy + rotMat[2]*vz;
	float viewDy = rotMat[4]*vx + rotMat[5]*vy + rotMat[6]*vz;
	float viewDz = rotMat[8]*vx + rotMat[9]*vy + rotMat[10]*vz;
	posx += viewDx;
	posy += viewDy;
	posz += viewDz;

	dirx = viewDx;
	diry = viewDy;
	dirz = viewDz;
	float rlenViewDir = 1.0f / sqrt( dirx*dirx + diry*diry + dirz*dirz );
	dirx *= rlenViewDir;
	diry *= rlenViewDir;
	dirz *= rlenViewDir;

	vx =0; vy = 0; vz = -1;
	dirx = rotMat[0]*vx + rotMat[1]*vy + rotMat[2]*vz;
	diry = rotMat[4]*vx + rotMat[5]*vy + rotMat[6]*vz;
	dirz = rotMat[8]*vx + rotMat[9]*vy + rotMat[10]*vz;

	// Translation matrix
	float tx[16] = {
	  1, 0, 0, 0,
	  0, 1, 0, 0,
	  0, 0, 1, 0,
	  -posx,  -posy,  -posz, 1
	};

	// Matrix mult, view = rx * translation
	modelview[ 0] = rotMat[0]*tx[ 0] + rotMat[4]*tx[ 1] + rotMat[ 8]*tx[ 2] + rotMat[12]*tx[ 3];
	modelview[ 1] = rotMat[1]*tx[ 0] + rotMat[5]*tx[ 1] + rotMat[ 9]*tx[ 2] + rotMat[13]*tx[ 3];
	modelview[ 2] = rotMat[2]*tx[ 0] + rotMat[6]*tx[ 1] + rotMat[10]*tx[ 2] + rotMat[14]*tx[ 3];
	modelview[ 3] = rotMat[3]*tx[ 0] + rotMat[7]*tx[ 1] + rotMat[11]*tx[ 2] + rotMat[15]*tx[ 3];

	modelview[ 4] = rotMat[0]*tx[ 4] + rotMat[4]*tx[ 5] + rotMat[ 8]*tx[ 6] + rotMat[12]*tx[ 7];
	modelview[ 5] = rotMat[1]*tx[ 4] + rotMat[5]*tx[ 5] + rotMat[ 9]*tx[ 6] + rotMat[13]*tx[ 7];
	modelview[ 6] = rotMat[2]*tx[ 4] + rotMat[6]*tx[ 5] + rotMat[10]*tx[ 6] + rotMat[14]*tx[ 7]; 
	modelview[ 7] = rotMat[3]*tx[ 4] + rotMat[7]*tx[ 5] + rotMat[11]*tx[ 6] + rotMat[15]*tx[ 7];

	modelview[ 8] = rotMat[0]*tx[ 8] + rotMat[4]*tx[ 9] + rotMat[ 8]*tx[10] + rotMat[12]*tx[11];
	modelview[ 9] = rotMat[1]*tx[ 8] + rotMat[5]*tx[ 9] + rotMat[ 9]*tx[10] + rotMat[13]*tx[11];
	modelview[10] = rotMat[2]*tx[ 8] + rotMat[6]*tx[ 9] + rotMat[10]*tx[10] + rotMat[14]*tx[11]; 
	modelview[11] = rotMat[3]*tx[ 8] + rotMat[7]*tx[ 9] + rotMat[11]*tx[10] + rotMat[15]*tx[11];
    
	modelview[12] = rotMat[0]*tx[12] + rotMat[4]*tx[13] + rotMat[ 8]*tx[14] + rotMat[12]*tx[15];
	modelview[13] = rotMat[1]*tx[12] + rotMat[5]*tx[13] + rotMat[ 9]*tx[14] + rotMat[13]*tx[15];
	modelview[14] = rotMat[2]*tx[12] + rotMat[6]*tx[13] + rotMat[10]*tx[14] + rotMat[14]*tx[15]; 
	modelview[15] = rotMat[3]*tx[12] + rotMat[7]*tx[13] + rotMat[11]*tx[14] + rotMat[15]*tx[15];
	// Modelview is ready to be loaded into opengl
}


float* PastyCam::getMatrix()
{
	return modelview;
}

void PastyCam::GetInverseMatrix(float *mCameraInverse)
{	
  // Below we do a little algebra to invert our current modelview matrix.
	// Instead of doing a more complicated method of determining the inverse
	// of a matrix, we can use this simplified version because we assume our
	// matrix is an orthogonal matrix, meaning that each column is perpendicular
	// to the other column.  What we do is remove the translation of the matrix
	// and then multiply it later by the negative translation, which reverses the
	// calculations of the original matrix.
	mCameraInverse[0]  = modelview[0]; mCameraInverse[1] = modelview[4]; mCameraInverse[2]  = modelview[8];
	mCameraInverse[4]  = modelview[1]; mCameraInverse[5] = modelview[5]; mCameraInverse[6]  = modelview[9];
	mCameraInverse[8]  = modelview[2]; mCameraInverse[9] = modelview[6]; mCameraInverse[10] = modelview[10];
	mCameraInverse[3]  = 0.0f; mCameraInverse[7] = 0.0f; mCameraInverse[11] = 0.0f;
	mCameraInverse[15] = 1.0f;

	mCameraInverse[12] = -(modelview[12] * modelview[0]) - (modelview[13] * modelview[1]) - (modelview[14] * modelview[2]);
	mCameraInverse[13] = -(modelview[12] * modelview[4]) - (modelview[13] * modelview[5]) - (modelview[14] * modelview[6]);
	mCameraInverse[14] = -(modelview[12] * modelview[8]) - (modelview[13] * modelview[9]) - (modelview[14] * modelview[10]);
}

void PastyCam::lookAt( float x, float y, float z )
{
	float identity[16] = {
		1,0,0,0,
		0,1,0,0,
		0,0,1,0,
		0,0,0,1,
	};
	for(int i=0; i<16; i++){
		modelview[i] = identity[i];
	}
	glusLookAtf(modelview,posx,posy,posz,x,y,z,0,1,0);
}
